#ifndef _BODYLINK_
#define _BODYLINK_

#include "BodyGen.h"
#include "Primitive.h"
#include "Marker.h"
#include <vector>
#include <vl/VLd.h>

#define BODYLINK_MAGIC_NO 500

using namespace std;

class Marker;
class Dof;
class Transformation;
class Joint;
class Model;

class BodyLink: public BodyGen{
protected:
	int mID;	// unique id
	int mModelID;	// location in the model
	Model *mModel;	// model to which it belongs
	char *mName;
	double mSubtreeMass;

	Primitive *mPrimitive;	// body geometry
	vector<Joint *> mChildJoints;	// list of joints that link to children
	Joint *mParentJoint;	// joint to connect to parent
	vector<Marker *> mHandles;	// list of handles associated
	void init();	// constructor code
	static int mBodyLinkCount;

public:
	BodyLink();
	BodyLink(char *_name);
	~BodyLink();

	Vec4d getWorldCoords(Vec4d *h);	// recursive function
	Mat4d getLocalTransform();
	Mat4d getLocalInvTransform();
	Mat4d getWorldTransform();
	Mat4d getWorldInvTransform();

	// get derivative wrt d
	Vec4d getFirstDerivative(Vec4d *h, Dof *d);
	Mat4d getFirstDerivative(Mat4d *h, Dof *d);
	Vec4d approxCOMFirstDerivative(Vec4d* h, Dof* d);
	Mat4d approxRotFirstDerivative(Mat4d* h, Dof* d);
	Mat4d getLocalFirstDerivative(Mat4d *h, Dof *d);
	// get second derivative wrt d1 and d2
	Vec4d getSecondDerivative(Vec4d *h, Dof *d1, Dof *d2);
	Mat4d getSecondDerivative(Mat4d *h, Dof *d1, Dof *d2);

	void setSubtreeMass();
	Vec3d computeSubtreeCOM();

	void draw(Vec4d _color, bool _default);	// render the entire bodylink subtree rooted here
	void drawHandles(Vec4d _color, bool _default);	// render the handles
	void setGLTransform(const double pos[3], const double R[12]);

	inline int getID(){return mID;}
	inline double getSubtreeMass(){ return mSubtreeMass; }	
	inline char* getName(){return mName;}
	inline int getModelID(){return mModelID;}
	inline void setModelID(int _m){mModelID=_m;}
	inline void addHandle(Marker *h){mHandles.push_back(h);}
	inline vector<Marker*> clearHandles(){
		vector<Marker*> ret = mHandles; 
		mHandles.clear(); 
		return ret;
	}
	inline void removeHandle(Marker *h){
		for(int i=0; i<mHandles.size(); i++){
			if(mHandles[i]==h){
				mHandles.erase(mHandles.begin()+i);
				break;
			}
		}
	}
	inline int getNumHandles(){return static_cast<int>(mHandles.size());}
	inline Marker* getHandle(int i){return mHandles[i];}
	inline Primitive* getPrimitive(){return mPrimitive;}
	inline void setPrimitive(Primitive *_p){
		mPrimitive = _p;
		mPrimitive->setColor(Vec3d(0.5, 0.5, 1.0));
		mPrimitive->setBody(this);
	}
	inline double getMass(){return mPrimitive->getMass();}
	inline Mat3d* getInertia() {return mPrimitive->getInertia();}
	inline Mat4d* getMassTensor(){return mPrimitive->getMassTensor();}
	inline void setMassTensorFromInertia(double mat[3][3]){mPrimitive->setMassTensorFromInertia(mat);}
	inline void addChildJoint(Joint *_c){mChildJoints.push_back(_c);}
	inline int getNumChildJoints(){return static_cast<int>(mChildJoints.size());}
	inline Joint* getChildJoint(int i){return mChildJoints[i];}
	inline Joint* getParentJoint(){return mParentJoint;}
	inline void setParentJoint(Joint *_p){mParentJoint=_p;}
	inline void setModel(Model *_m){mModel=_m;}
	inline Model *getModel(){return mModel;}

	bool *dependsOnDof;	// map to answer the question whether the bodylink depends on the asked dof or not.

	// not initialized automatically
	BodyLink *mOtherSideLink;	// corresponding link on the other side of body if any
};

#endif
